#include "schedule.h"
#include "led.h"
#include "key.h"
#include "tb6612fng.h"

static u8 led_cnt;
int textSpd[4] = {2048,2048,2048,2048};

void Loop_1000Hz(void)  //1ms
{
    
}

void Loop_500Hz(void)   //2ms
{
}

void Loop_200Hz(void)   //5ms
{
	
}

void Loop_100Hz(void)   //10ms
{
    
}

void Loop_50Hz(void)    //20ms
{
    Chassis_onforce();
    Chassis_SpeedSet(textSpd);
}

void Loop_20Hz(void)    //50ms
{
	if(led_cnt++>6)
	{
		LED_TOGGLE;
		led_cnt = 0;
	}
}

void Loop(schedule* robotSchdule)
{
    if(robotSchdule->cnt_1ms >= 1)
    {
        Loop_1000Hz();
        robotSchdule->cnt_1ms = 0;
    }
    if(robotSchdule->cnt_2ms >= 2)
    {
        Loop_500Hz();
        robotSchdule->cnt_2ms = 0;
    }
    if(robotSchdule->cnt_5ms >= 5)
    {
        Loop_200Hz();
        robotSchdule->cnt_5ms = 0;
    }
    if(robotSchdule->cnt_10ms >= 10)
    {
        Loop_100Hz();
        robotSchdule->cnt_10ms = 0;
    }
    if(robotSchdule->cnt_20ms >= 20)
    {
        Loop_50Hz();
        robotSchdule->cnt_20ms = 0;
    }
    if(robotSchdule->cnt_50ms >= 50)
    {
        Loop_20Hz();
        robotSchdule->cnt_50ms = 0;
    }
}


